#ifndef CONTROL_H
#define CONTROL_H

#include "motion9_dll.h"
#include "./trajectory_plane/tp_type.h"
#include "platform.h"
#include "./joint_controller/joint_controller.h"
#include "position_out.h"

/*
--------------控制debug控制----------------
*/
#define CONTROLLER_DEBUG
#ifdef DEBUG
#ifdef WIN32
#define DLog(s1,...) printf(WHITE"[log]:func:%s, line:%d " s1"\n"NONE,__FUNCTION__, __LINE__, ##__VA_ARGS__)
#else
#define DLog(s1,...) printf(WHITE"[log]:func:%s, line:%d " s1"\n"NONE,__func__, __LINE__, ##__VA_ARGS__)
#endif
#else
#define DLog(s1,...)
#endif

#define CONTROLLER_ERROR
#ifdef DEBUG
#ifdef WIN32
#define DError(s1,...) printf(YELLOW"[error]:func:%s, line:%d " s1"\n"NONE,__FUNCTION__, __LINE__, ##__VA_ARGS__)
#else
#define DError(s1,...) printf(YELLOW"[error]:func:%s, line:%d "  s1"\n"NONE,__func__, __LINE__, ##__VA_ARGS__)
#endif
#else
#define DError(s1,...)
#endif

#define CONTROLLER_WARN
#ifdef DEBUG
#ifdef WIN32
#define DWarn(s1,...) printf(RED"[warn]:func:%s, line:%d " s1"\n"NONE,__FUNCTION__, __LINE__, ##__VA_ARGS__)
#else
#define DWarn(s1,...) printf(RED"[warn]:func:%s, line:%d "  s1"\n"NONE,__func__, __LINE__, ##__VA_ARGS__)
#endif
#else
#define DWarn(s1,...)
#endif

#define ARC_BLEND_LOG
#ifdef ARC_BLEND_LOG
#ifdef WIN32
#define AbLog(s1,...) printf(WHITE"[log]:func:%s, line:%d " s1"\n"NONE,__FUNCTION__, __LINE__, ##__VA_ARGS__)
#else
#define AbLog(s1,...) printf(WHITE"[log]:func:%s, line:%d " s1"\n"NONE,__func__, __LINE__, ##__VA_ARGS__)
#endif
#else
#define AbLog(s1,...)
#endif

//---------------------------------------------
/*
Controller struct
控制器结构体
*/
typedef enum{
	EM_INTER_IDLE = 0,
	EM_INTER_PLANE = 1,
	EM_INTER_FREE = 2,
}ENUM_INTER_MODE;

typedef struct{
	char init_done;
	STRUCT_MOTION_PARM conf;
	ENUM_CMD_STATUS cmd_status;
	ENUM_INTER_MODE inter_mode;
	float joint_max_vel_rate[MAX_JOINT];
	float joint_max_acc_rate[MAX_JOINT];

	//---------------------
	double curr_posit[MAX_JOINT];		//Joint Position
	double curr_posit_l[MAX_JOINT];		//Joint Position
	float curr_vel[MAX_JOINT];			//Joint Velocity
	float curr_vel_l[MAX_JOINT];		//Joint Velocity
	float curr_acc[MAX_JOINT];			//Joint Acceleration
	double feedback_posit[MAX_JOINT];	//Feedback
	STRUCT_FR fr;
	STRUCT_TP tp;
	STRUCT_TC tc_queue_space[TCQ_QUEUE_LEN + 10];
	STRUCT_JOINT_CONTROL joint_control;
	STRUCT_PO s_po;

	//---------------------
	float feed_rate;		//进给率
	char aborting;
	char pausing;
	char joint_busy_flag[MAX_JOINT];
	char running;
	int execute_id;
	char soft_limit_flag;
	char abs_limit_flag;
}STRUCT_CONTROL;


//=================API============================
int Controller_RunCycle();
int Controller_Initial();
int Controller_Clear();
int Controller_GetNextPoint(double posit[MAX_JOINT]);

//control check
int Controller_CheckInitialDone();
int Controller_CheckEmergency();
int Controller_CheckPause();
int Controller_CheckAbort();
int Controller_CheckInterModePlane(int id);
int Controller_CheckInterModeFree(int id);
int Controller_CheckPlaneErrorOccur();

//update state
int Controller_UpdateState();

//================================================
void Controller_Print(const char *format, ...);
void Controller_MsgVp(int reason, const char *format, ...);
//void Controller_Msg(int reason, int len, char *msg);

#endif
